//
// Created by Pulsar on 2020/5/18.
//
#ifdef WITH_LIBREALSENCE2
#include <librealsense2/rs.hpp>
#include <opencv2/opencv.hpp>
#include <iostream>


#define WIDTH 640
#define HEIGHT 480
#define FPS 30

using namespace cv;

int main(int argc, char **argv) {
    rs2::context ctx;

    std::cout << "hello from librealsense - " << RS2_API_VERSION_STR << std::endl;
    std::cout << "You have " << ctx.query_devices().size() << " RealSense devices connected" << std::endl;


    rs2::pipeline pipe;

    rs2::config cfg;//创建一个以非默认配置的配置用来配置管道
    //Add desired streams to configuration
    cfg.enable_stream(RS2_STREAM_COLOR, WIDTH, HEIGHT, RS2_FORMAT_BGR8, FPS);//向配置添加所需的流
    cfg.enable_stream(RS2_STREAM_DEPTH, WIDTH, HEIGHT, RS2_FORMAT_Z16,FPS);
    pipe.start(cfg);

    while (true) {
        rs2::frameset frames = pipe.wait_for_frames();

        rs2::depth_frame depth = frames.get_depth_frame();
        rs2::frame color = frames.get_color_frame();


        Mat depth_image(Size(WIDTH, HEIGHT), CV_16U, (void *) depth.get_data(), Mat::AUTO_STEP);
        Mat depth_image_8U;
        depth_image.convertTo(depth_image_8U,CV_8UC3);
//        Mat im_color;
//        applyColorMap(depth_image_8U, im_color, COLORMAP_JET);

        Mat color_image(Size(WIDTH, HEIGHT), CV_8UC3, (void *) color.get_data(), Mat::AUTO_STEP);


//        cv::imshow("depth_image", depth_image);
//        cv::imshow("color_image", color_image);
        imwrite("temp.png", color_image);
        if (waitKey(100) == 'q')break;

        float dist_to_center = depth.get_distance(WIDTH / 2, HEIGHT / 2);

        std::cout << "The camera is facing an object " << dist_to_center << " meters away \n";
        break;
    }
    return 0;
}

#else
int main(int argc,char** argv){

    return 0;
}
#endif